#include "pinctrl.h"
#include "i2c.h"
#include "soc_osal.h"
#include "app_init.h"
#include <stdint.h>
#include "gpio.h"
#include <stdio.h>
#include <stdlib.h> // 包含stdlib.h以使用malloc和free

// #ifndef __MPU6050_H__
// #include "mpu6050.h"
// #define __MPU6050_H__
// #endif

#ifndef MPU6050_H
#define MPU6050_H
#include "mpu6050.h"
#endif

// Example data points representing standard gestures (simplified for demonstration)

#define NUM_FEATURES 6             // Number of features (3 Acc + 3 Gyro)
#define NUM_SAMPLES_PER_GESTURE 40 // Number of samples to average for each gesture
#define THRESHOLD 150              // Threshold to determine if the gesture matches

void readSensorData(SensorData *data);
void calculate_accel_angle(float AccX, float AccY, float AccZ);
void update_gyro_angle(float GyroX, float GyroY, float GyroZ, float dt);
void kalman_filter_init(kalman_filter_t *kf, float q, float r, float x_init, float p_init);
float kalman_filter_update(kalman_filter_t *kf, float measurement);
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data);
uint8_t MPU6050_ReadReg(uint8_t RegAddress);
void MPU6050_Init(void);
void MPU6050_GetData(SensorData *data);
return_of_angle_Recognition_Low angle_Recognition_Low(void);

long double zero_drift_GyroX_long_double=0.0;  //X方向的陀螺仪数据校准值
long double zero_drift_GyroY_long_double=0.0;  //Y方向的陀螺仪数据校准值
long double zero_drift_GyroZ_long_double=0.0;  //Z方向的陀螺仪数据校准值

long double zero_drift_GyroX=0.0;  //X方向的陀螺仪数据校准值
long double zero_drift_GyroY=0.0;  //Y方向的陀螺仪数据校准值
long double zero_drift_GyroZ=0.0;  //Z方向的陀螺仪数据校准值

static char ch_print_roll[100];
static char ch_print_pitch[100];

// 手势识别程序
void readSensorData(SensorData *data)
{
    MPU6050_GetData(data);
}

void calculate_accel_angle(float AccX, float AccY, float AccZ)
{
    // 计算角度
    // static char ch_print_roll[100];
    // static char ch_print_pitch[100];
    memset(ch_print_roll,0,sizeof(ch_print_roll));
    memset(ch_print_pitch,0,sizeof(ch_print_roll));
    current_angle.roll = atan2((double)AccX, sqrt(AccY * AccY + AccZ * AccZ)) * 180.0f / M_PI;
    current_angle.pitch = atan2((double)(-AccY), (double)AccZ) * 180.0f / M_PI;
    snprintf(ch_print_roll, sizeof(ch_print_roll), "%.2f", current_angle.roll);
    snprintf(ch_print_pitch, sizeof(ch_print_pitch), "%.2f", current_angle.pitch);
    printf("current_angle.roll is %s,current_angle.pitch is %s\n",ch_print_roll,ch_print_roll);
}

// 从陀螺仪数据更新角度
void update_gyro_angle(float GyroX, float GyroY, float GyroZ, float dt)
{
    unused(GyroX);
    unused(GyroY);
    unused(GyroZ);
    // 积分得到角度
    // current_angle.pitch += GyroX * dt*100;
    // current_angle.roll += GyroY * dt*100;
    current_angle.yaw += GyroZ * dt*100;
}


// 卡尔曼滤波
void kalman_filter_init(kalman_filter_t *kf, float q, float r, float x_init, float p_init)
{
    kf->Q = q;
    kf->R = r;
    kf->P = p_init;
    kf->K = 0;
    kf->X = x_init;
}

float kalman_filter_update(kalman_filter_t *kf, float measurement)
{
    float innovation = measurement - kf->X;
    kf->K = kf->P / (kf->P + kf->R);
    kf->X += kf->K * innovation;
    kf->P = (1 - kf->K) * kf->P + kf->Q;
    return kf->X;
}

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    i2c_data_t mpudata = {0};
    uint8_t databuff[2] = {RegAddress, Data};
    mpudata.send_buf = databuff;  //存储待发送的数据
    mpudata.send_len = sizeof(databuff);
    uint32_t ret = uapi_i2c_master_write(CONFIG_I2C_MASTER_BUS_ID, MPU6050_ADDRESS, &mpudata);
    if (ret != ERRCODE_SUCC)
    {
        osal_printk("i2c%d master send error %x !\r\n", CONFIG_I2C_MASTER_BUS_ID, ret);
    }
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
    i2c_data_t mpudata1 = {0};
    uint8_t databuff[2] = {0};
    mpudata1.send_buf = &RegAddress;
    mpudata1.send_len = sizeof(RegAddress);
    mpudata1.receive_buf = databuff;
    mpudata1.receive_len = sizeof(databuff);
    uint32_t ret = uapi_i2c_master_writeread(CONFIG_I2C_MASTER_BUS_ID, MPU6050_ADDRESS, &mpudata1);
    if (ret != ERRCODE_SUCC)
    {
        osal_printk("i2c%d master read error %x!\r\n", CONFIG_I2C_MASTER_BUS_ID, ret);
    }
    return databuff[0];
}

void MPU6050_Init(void)
{
    /*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
    MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0X80); // 复位MPU6050
    printf("fuwei wancheng\n");
    osal_udelay(20000);
    MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0X00);  // 唤醒MPU6050
    printf("wake up complete\n");
    MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0X00);  // 关闭所有中断
    MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0X00);   // I2C主模式关闭
    MPU6050_WriteReg(MPU6050_RA_FIFO_EN, 0X00);     // 关闭FIFO
    MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0X80); // 中断的逻辑电平模式,设置为0，中断信号为高电；设置为1，中断信号为低电平时。
    // MotionInterrupt();                              // 运动中断
    MPU6050_WriteReg(MPU6050_RA_CONFIG, 0x04);       // 配置外部引脚采样和DLPF数字低通滤波器
    MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x1C); // 加速度传感器量程和高通滤波器配置
    MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0X1C);  // INT引脚低电平平时
    MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x40);   // 中断使能寄存器

    printf("---------------------zero_drift correct begin---------------------\n");
    SensorData *motion1Samples = (SensorData *)malloc(NUM_SAMPLES_PER_GESTURE * sizeof(SensorData));        //申请内存空间
    // 获取时间间隔
    static float dt = I2C_TASK_DURATION_MS / 1000.0f;
    unused(dt);
    static int drift_times = 100;      //进行零漂的取平均计算次数
    static char ch_zero_drift_GyroX[100];   //用来存放X方向的角速度零漂
    static char ch_zero_drift_GyroY[100];   //用来存放Y方向的角速度零漂
    static char ch_zero_drift_GyroZ[100];   //用来存放Z方向的角速度零漂
    
    drift_times = 2000;
    if (motion1Samples == NULL)         //说明内存空间申请失败
    {
        printf("Memory allocation failed for motion1Samples\n");
        return ; // 返回错误代码
    }
    for(int i=1;i<=drift_times;i++){
        readSensorData(motion1Samples);
        zero_drift_GyroX_long_double = zero_drift_GyroX_long_double+(motion1Samples->gyro_x);
        zero_drift_GyroY_long_double = zero_drift_GyroY_long_double+(motion1Samples->gyro_y);
        zero_drift_GyroZ_long_double = zero_drift_GyroZ_long_double+(motion1Samples->gyro_z);
        if(i%500==0){
            printf("i is %d\n",i);
        }
        // printf("zero_drift_GyroX is %.6f,zero_drift_GyroY is %.6f,zero_drift_GyroZ is %.6f\n",zero_drift_GyroX,zero_drift_GyroY,zero_drift_GyroZ);
    }
    // printf("motion1Samples->gyro_x is %.2f\n",motion1Samples->gyro_x);
    printf("for is over\n");

    zero_drift_GyroX_long_double/=drift_times;      //得到X方向上角速度零漂补偿值,在使用角速度时应该减去这个数
    zero_drift_GyroY_long_double/=drift_times;      //得到Y方向上角速度零漂补偿值,在使用角速度时应该减去这个数
    zero_drift_GyroZ_long_double/=drift_times;      //得到Z方向上角速度零漂补偿值,在使用角速度时应该减去这个数

    zero_drift_GyroX=(double)zero_drift_GyroX_long_double;      //进行强制类型转换,变为double类型
    zero_drift_GyroY=(double)zero_drift_GyroY_long_double;      //进行强制类型转换,变为double类型
    zero_drift_GyroZ=(double)zero_drift_GyroZ_long_double;      //进行强制类型转换,变为double类型
    snprintf(ch_zero_drift_GyroX, sizeof(ch_zero_drift_GyroX), "%.2f", zero_drift_GyroX);
    printf("ch_zero_drift_GyroX is %s\n",ch_zero_drift_GyroX);
    snprintf(ch_zero_drift_GyroY, sizeof(ch_zero_drift_GyroY), "%.2f", zero_drift_GyroY);
    printf("ch_zero_drift_GyroY is %s\n",ch_zero_drift_GyroY);
    snprintf(ch_zero_drift_GyroZ, sizeof(ch_zero_drift_GyroZ), "%.2f", zero_drift_GyroZ);
    printf("ch_zero_drift_GyroZ is %s\n",ch_zero_drift_GyroZ);
    
    // printf("zero_drift_GyroX is %.6f,zero_drift_GyroY is %.6f,zero_drift_GyroZ is %.6f\n",zero_drift_GyroX,zero_drift_GyroY,zero_drift_GyroZ);
    printf("---------------------zero_drift correct end---------------------\n");

    printf("MPU6050_Init complete\n");
    return ;
}


void MPU6050_GetData(SensorData *data)
{
    uint8_t DataH, DataL; // 定义数据高8位和低8位的变量

    // 初始化Kalman滤波器
    kalman_filter_t kf_acc_x, kf_acc_y, kf_acc_z;
    kalman_filter_t kf_gyro_x, kf_gyro_y, kf_gyro_z;
    // printf("---------------------MPU6050_GetData begin---------------------\n");
    // 例如，Q = 0.001, R = 0.03, X = 初始估计值, P = 初始协方差估计值
    kalman_filter_init(&kf_acc_x, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_acc_y, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_acc_z, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_x, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_y, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_z, 0.001, 0.03, 0.0, 1.0);

    // 读取加速度计和陀螺仪数据
    DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);                  // 读取加速度计X轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);                  // 读取加速度计X轴的低8位数据
    AccX = (DataH << 8) | DataL;                                    // 数据拼接
    printf("AccX is %d\n",AccX);
    data->acc_x = kalman_filter_update(&kf_acc_x, AccX / 16384.0f); // 应用Kalman滤波器并转换单位
    // data->acc_x = kalman_filter_update(&kf_acc_x, AccX / 163.0f); // 应用Kalman滤波器并转换单位

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);                  // 读取加速度计Y轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);                  // 读取加速度计Y轴的低8位数据
    AccY = (DataH << 8) | DataL;                                    // 数据拼接
    printf("AccY is %d\n",AccY);
    data->acc_y = kalman_filter_update(&kf_acc_y, AccY / 16384.0f); // 应用Kalman滤波器并转换单位
    // data->acc_y = kalman_filter_update(&kf_acc_y, AccY / 163.0f); // 应用Kalman滤波器并转换单位

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);                  // 读取加速度计Z轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);                  // 读取加速度计Z轴的低8位数据
    AccZ = (DataH << 8) | DataL;                                    // 数据拼接
    printf("AccZ is %d\n",AccZ);
    data->acc_z = kalman_filter_update(&kf_acc_z, AccZ / 16384.0f); // 应用Kalman滤波器并转换单位
    // data->acc_z = kalman_filter_update(&kf_acc_z, AccZ / 163.0f); // 应用Kalman滤波器并转换单位

    DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);                    // 读取陀螺仪X轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);                    // 读取陀螺仪X轴的低8位数据
    GyroX = (DataH << 8) | DataL;                                    // 数据拼接
    // printf("GyroX is %d\n",GyroX);
    data->gyro_x = kalman_filter_update(&kf_gyro_x, (float)GyroX / 131.0f); // 应用Kalman滤波器并转换单位

    DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);                    // 读取陀螺仪Y轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);                    // 读取陀螺仪Y轴的低8位数据
    GyroY = (DataH << 8) | DataL;                                    // 数据拼接
    // printf("GyroY is %d\n",GyroY);
    data->gyro_y = kalman_filter_update(&kf_gyro_y, (float)GyroY / 131.0f); // 应用Kalman滤波器并转换单位

    DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);                    // 读取陀螺仪Z轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);                    // 读取陀螺仪Z轴的低8位数据
    GyroZ = (DataH << 8) | DataL;                                    // 数据拼接
    // printf("GyroZ is %d\n",GyroZ);
    data->gyro_z = kalman_filter_update(&kf_gyro_z, (float)GyroZ / 131.0f); // 应用Kalman滤波器并转换单位

    static char kalman_strings[6][20];
    memset(kalman_strings,0,sizeof(kalman_strings));
    snprintf(kalman_strings[0], sizeof(kalman_strings[0]), "%.2f", data->acc_x);
    snprintf(kalman_strings[1], sizeof(kalman_strings[1]), "%.2f", data->acc_y);
    snprintf(kalman_strings[2], sizeof(kalman_strings[2]), "%.2f", data->acc_z);
    snprintf(kalman_strings[3], sizeof(kalman_strings[3]), "%.2f", data->gyro_y);
    snprintf(kalman_strings[4], sizeof(kalman_strings[4]), "%.2f", data->gyro_x);
    snprintf(kalman_strings[5], sizeof(kalman_strings[5]), "%.2f", data->gyro_z);
    // printf("kalman_strings[0] is %s\n",kalman_strings[0]);
    // 输出格式化后的字符串
    // printf("MPU6050_Data=kalman_acc_x:%s,kalman_acc_y:%s,kalman_acc_z:%s,kalman_gyro_x:%s,kalman_gyro_y:%s,kalman_gyro_z:%s\r\n",
    //        kalman_strings[0], kalman_strings[1], kalman_strings[2],
    //        kalman_strings[3], kalman_strings[4], kalman_strings[5]);

    
    // printf("---------------------MPU6050_GetData end---------------------\n");
}


return_of_angle_Recognition_Low angle_Recognition_Low(void)
{
    char angle_strings[3][20];
    static return_of_angle_Recognition_Low tmp;
    tmp.angle_1=0.0;
    tmp.angle_2=0.0;
    tmp.angle_3=0.0;

    // uapi_tcxo_delay_count(10);  //等待10个ticks    
    SensorData *motion1Samples = (SensorData *)malloc(NUM_SAMPLES_PER_GESTURE * sizeof(SensorData));        //申请内存空间
    if (motion1Samples == NULL)         //说明内存空间申请失败
    {
        printf("Memory allocation failed for motion1Samples\n");
        return tmp; // 返回错误代码
    }
    // uapi_tcxo_delay_count(10);  //等待10个ticks    
    for (size_t i = 0; i <= 0; i++)
    {
        // uapi_tcxo_delay_count(10);  //等待10个ticks    
        // if(i%10==0) printf("come into the for loop,i is %d\n",i);
        printf("---------------------in the for,readSensorData begin---------------------\n");
        readSensorData(motion1Samples);
        printf("---------------------in the for,readSensorData end---------------------\n");
        printf("readSensorData complete\n");
        // 转换加速度计数据为角度
        calculate_accel_angle(motion1Samples->acc_x, motion1Samples->acc_y, motion1Samples->acc_z);
        printf("calculate_accel_angle complete\n");
        // 获取时间间隔
        float dt = I2C_TASK_DURATION_MS / 1000.0f;

        // 使用陀螺仪数据更新角度
        static char motion1Samples_gyro_x[100];
        static char motion1Samples_gyro_y[100];
        static char motion1Samples_gyro_z[100];
        memset(motion1Samples_gyro_x,0,sizeof(motion1Samples_gyro_x));
        memset(motion1Samples_gyro_y,0,sizeof(motion1Samples_gyro_y));
        memset(motion1Samples_gyro_z,0,sizeof(motion1Samples_gyro_z));
        motion1Samples->gyro_x -= zero_drift_GyroX;
        snprintf(motion1Samples_gyro_x, sizeof(motion1Samples_gyro_x), "%.2f",motion1Samples->gyro_x);
        // printf("motion1Samples_gyro_x is %s\n",motion1Samples_gyro_x);
        motion1Samples->gyro_y -= zero_drift_GyroY;
        snprintf(motion1Samples_gyro_y, sizeof(motion1Samples_gyro_y), "%.2f",motion1Samples->gyro_y);
        // printf("motion1Samples_gyro_y is %s\n",motion1Samples_gyro_y);
        motion1Samples->gyro_z -= zero_drift_GyroZ;
        motion1Samples->gyro_z -= 0.2155;
        snprintf(motion1Samples_gyro_z, sizeof(motion1Samples_gyro_z), "%.2f",motion1Samples->gyro_z);
        // printf("motion1Samples_gyro_z is %s\n",motion1Samples_gyro_z);

        //使用陀螺仪角速度,通过积分更新ypr角
        update_gyro_angle(motion1Samples->gyro_x, motion1Samples->gyro_y, motion1Samples->gyro_z, dt);
        // printf("update_gyro_angle complete\n");

        snprintf(angle_strings[0], sizeof(angle_strings[0]), "%.2f", current_angle.pitch);//将 current_angle.pitch（一个浮动值，表示当前的俯仰角度）格式化为一个字符串，并将其存储在 angle_strings[0] 中
        snprintf(angle_strings[1], sizeof(angle_strings[1]), "%.2f", current_angle.roll);
        snprintf(angle_strings[2], sizeof(angle_strings[2]), "%.2f", current_angle.yaw);

        if(i%2==0) printf("Angle_Data1 = %s,Angle_Data2 = %s,Angle_Data3 = %s\r\n", angle_strings[0], angle_strings[1], angle_strings[2]);
    }

    free(motion1Samples); // 释放动态分配的内存
    tmp.angle_1=current_angle.pitch;
    tmp.angle_2=current_angle.roll;
    tmp.angle_3=current_angle.yaw;

    return tmp;
}